# -*- coding: UTF-8 -*-

import rospy
from cv_bridge import CvBridge
from cv_bridge.boost.cv_bridge_boost import getCvType
from sensor_msgs.msg import Image, PointCloud2
from sensor_msgs import point_cloud2
from qomolo_msgs.msg import Gps
from nav_msgs.msg import Odometry
import message_filters

import sys
import cv2
import time
import os
import shutil
import numpy as np


save_dir = ''
last_gps_save_time = 0
last_fusion_save_time = 0 
fusion_f = None
gps_f = None


def gps_odom_call(gps_data):
    global last_gps_save_time, save_dir, gps_f
    second = float(gps_data.header.stamp.secs)
    second_fraction = float(gps_data.header.stamp.nsecs)/1e9
    timestamp = second + second_fraction
    save_name = second
    
    if (second_fraction >= 0 and second_fraction < 0.5):
        save_name = second + 0.25
    else:
        save_name = second + 0.75
    
    # print("image timestamp", timestamp)
    
    time_diff = abs(timestamp-save_name)
    if time_diff < abs(last_gps_save_time-save_name) and time_diff < 0.05:  


        info = ','.join((str(gps_data.pose.pose.position.x) , str(gps_data.pose.pose.position.y) , str(gps_data.pose.pose.position.z) , str(gps_data.pose.pose.orientation.x) , str(gps_data.pose.pose.orientation.y) , str(gps_data.pose.pose.orientation.z),str(gps_data.pose.pose.orientation.w)))
        gps_str = str(int(save_name*1e6)) + "_" + str(int(timestamp*1e6)) + ":" + info + "\r"
        # print(gps_str)
        
        gps_f.write(gps_str)
        last_gps_save_time = timestamp
        

        
        
def fusion_odom_call(fusion_data):
    global last_fusion_save_time, save_dir, fusion_f
    second = float(fusion_data.header.stamp.secs)
    second_fraction = float(fusion_data.header.stamp.nsecs)/1e9
    timestamp = second + second_fraction
    save_name = second
    
    if (second_fraction >= 0 and second_fraction < 0.5):
        save_name = second + 0.25
    else:
        save_name = second + 0.75
    
    # print("image timestamp", timestamp)
    
    time_diff = abs(timestamp-save_name)
    if time_diff < abs(last_fusion_save_time-save_name) and time_diff < 0.05:  


        info = ','.join((str(fusion_data.pose.pose.position.x) , str(fusion_data.pose.pose.position.y) , str(fusion_data.pose.pose.position.z) , str(fusion_data.pose.pose.orientation.x) , str(fusion_data.pose.pose.orientation.y) , str(fusion_data.pose.pose.orientation.z),str(fusion_data.pose.pose.orientation.w)))
        fusion_str = str(int(save_name*1e6)) + "_" + str(int(timestamp*1e6)) + ":" + info + "\r"
        # print(gps_str)
        
        fusion_f.write(fusion_str)
        last_fusion_save_time = timestamp
        

    
if __name__ == "__main__":
    save_dir = sys.argv[1]
    print(save_dir)
    if not os.path.exists(save_dir):
        os.makedirs(save_dir)
        os.makedirs(save_dir + "/lidar")
        os.makedirs(save_dir + "/image")
        os.makedirs(save_dir + "/image/front_left")
        os.makedirs(save_dir + "/image/front_mid")
        os.makedirs(save_dir + "/image/front_right")
        os.makedirs(save_dir + "/image/rear_left")
        os.makedirs(save_dir + "/image/rear_right")
        os.makedirs(save_dir + "/image/top_left")
        os.makedirs(save_dir + "/image/top_right")
    gps_f = open(os.path.join(save_dir, 'gps_odom.txt'), 'w')
    fusion_f = open(os.path.join(save_dir, 'fusion_odom.txt'), 'w')

    print("init node")
    rospy.init_node("save_odom", anonymous=True)
    rospy.Subscriber("/gps_odom", Odometry, gps_odom_call, queue_size=10)
    rospy.Subscriber("/fusion_odom", Odometry, fusion_odom_call, queue_size=10)
    rospy.spin()
    
